#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <gazebo_msgs/ModelStates.h>

#include<iostream>
#include<string>

class gazebo_model_pose
{
private:
    ros::NodeHandle nh_;
    ros::Publisher model_pose_pub_;
    ros::Subscriber gazebo_modelstates_sub_;

    std::string model_name_;

public:
    gazebo_model_pose(ros::NodeHandle &nh)
    {
        nh_ = nh;
        nh_.getParam("model_name_", model_name_);
        std::cout << "\033[34m[gazebo_model_pose_pub] Model name is:\033[0m" << model_name_ << "." << std::endl;
        model_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("gazebo_model_pose_topic", 100);
        gazebo_modelstates_sub_ = nh_.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 100, &gazebo_model_pose::RcvModelStatesCallback, this);
    }    
    ~gazebo_model_pose(){}

    void RcvModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
    {
        geometry_msgs::PoseStamped model_pose;
        int model_cnt = msg->name.size();
        // "for" in invert order will be faster
        for (int i = model_cnt-1; i >= 0; i--)
        {
            if (msg->name[i] == model_name_)
            {
                model_pose.header.frame_id = "world";
                model_pose.header.stamp = ros::Time::now();
                model_pose.pose.position = msg->pose[i].position;
                model_pose.pose.orientation = msg->pose[i].orientation;
                model_pose_pub_.publish(model_pose);
                break;
            }
        }
    }

};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "gazebo_model_pose_pub");
    ros::NodeHandle nh("~");

    gazebo_model_pose intermediary(nh);

    ros::spin();
    // ros::Rate loop_rate(30);
    // while (ros::ok())
    // {
    //     ros::spinOnce();
    //     loop_rate.sleep(); 
    // }

    return 0;

}